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параметры Родрига-Гамильтона В 


результат вычислений 


компонентах 
кватерниона, передается в 
программный пакет МаНаБ, выполняющий программу для 
визуализации зависимостей от времени четырёх 
компонент кватерниона в виде графиков. 

Результаты исследования. В существующих алгоритмах, 
использующих параметры Родрига-Гамильтона, при 
начальной инициализации системы ориентации авторами 
предложено увеличивать значение коэффициента 
коррекции на время инициализации, либо с помощью 
тригонометрических формул находить углы Эйлера и 
переводить их в параметры Родрига-Гамильтона. В 
первом случае время начальной инициализации остается 
достаточно большим, во втором случае, из-за 
использования углов Эйлера, может возникать такое 
явление как «шарнирный замок». Предложенный в данной 
работе алгоритм осуществляет начальную инициализацию 
за время, сравнимое со временем инициализации в углах 
Эйлера, но при этом использует только параметры 
Родрига-Гамильтона. 

Обсуждение и заключения. Использование предложенного 
алгоритма позволит минимум в 5 раз сократить время 
начальной инициализации кватерниона пространственной 
ориентации и, как следствие, общее время, необходимое 
для приведения системы в рабочее состояние, за счет того, 
что начальная инициализация необходима при каждом 
включении системы ориентации. В силу того, что 
инициализация происходит только по показаниям 
акселерометра, то для корректного определения 
пространственной ориентации по — предложенному 
алгоритму необходимым условием является отсутствие 
любых ускорений на тело кроме ускорения свободного 
падения. 


Ключевые слова: кватернион, параметры Родрига- 
Гамильтона, приборный базис, коррекция, гироскопы, 
акселерометры, плоскость приведения. 
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Введение. В задачах определения пространственной ориентации твердого тела относительно опорной 


системы координат широко применяются бесплатформенные инерциальные навигационные системы. В 


современных системах, к которым предъявляют жесткие требования по массогабаритным показателям, при 


определении пространственной ориентации в качестве сенсоров используются измеритель проекции угловой 
скорости на ось чувствительности (далее такие сенсоры будем называть гироскопами), измерители проекции 


ускорения на ось чувствительности (акселерометры) и измеритель проекции вектора магнитного поля 


(магнитный компас). В общем случае при определении пространственной ориентации в качестве основного 


источника информации используют интегрированные показания от трех датчиков угловой скорости, 


расположенных взаимно-перпендикулярно. С ПОМОЩЬЮ информации, получаемой от акселерометра и 


магнитного компаса, оси чувствительности которых расположены так же, как и оси гироскопа, сводят к 


минимуму возникающую в процессе интегрирования угловой скорости ошибку, которая есть разница между 
реальной пространственной ориентацией и пространственной ориентацией, вычисленной с помощью датчиков 


угловой скорости[1]. Существует несколько кинематических параметров для описания движения твердого тела 


около неподвижной точки [2]: 
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® Углы Эйлера-Крылова; 

® Матрица направляющих косинусов; 

® параметры Родрига-Гамильтона (в компонентах кватерниона). 
® параметры Кейли — Клейнав. 


Применение кватернионов позволяет создать удобный и наглядный формализм, использующий 
параметры Родрига-Гамильтона для описания пространственной ориентации твердого тела, что является 
наиболее близким к оптимальному с точки зрения вычислительной техники [2,3]. При описании 
пространственной ориентации удобно выделять три базиса: 


® Опорный базис [, неподвижный в инерциальном пространстве, представленный в виде единичного 
кватерниона на рис. 1; 

® Приборный базис [* — оцененная с помощью датчиков пространственная ориентация объекта; 

® Связанный базис Ё — базис, связанный с объектом ориентации. 


Связанный базис 








Рис. 1. Расположение систем координат в связанном и опорном базисах 
Ето. 1. Госапоп орсоот@теие зуяет5 т оса апа "о! р’атез 


Кватернионом называется упорядоченная четверка действительных чисел: 
А = (№, № № №), (1) 
где Ло — скалярная часть; А1, А2, Аз — векторная часть. 
Единичный кватернион представлен в виде: 
Л = (1, 0, 0, 0.. (2) 
Кватернион, описывающий пространственную ориентацию твердого тела, есть кватернион, 
описывающий поворот относительно единичного кватерниона. Из-за несовершенства датчиков угловой 
скорости, конечной разрядности чисел при представлении на ЭВМ с течением времени накапливается ошибка, 
которую можно выразить как разницу между опорным базисом и приборным. Для приведения приборного 
базиса к опорному (1 > Г) коррекция должна осуществляться через изменение кватерниона поворота ДЛ, 
который находится путем интегрирования измеренной датчиком угловой скорости. При таком методе 
коррекции время приведения приборного базиса к опорному прямо пропорциональна ошибке между этими 
базисами. В момент включения системы ориентации необходимо за минимальный промежуток времени свести 
ошибку между опорным и приборным базисами к минимуму (произвести «выставку» системы ориентации). В 
работе [4] автор на время «выставки» предлагает увеличить коэффициент коррекции через который 
осуществляется приведение приборного базиса к опорному. Под коэффициентом коррекции понимается число, 
на которое умножаются компоненты коррекции перед изменением кватерниона поворота ДЛ. С учетом 
увеличения коэффициента коррекции «выставка» занимает время от 100 до 3000 итераций в зависимости от 
значения коэффициента коррекции и начальной пространственной ориентации системы (под итерацией 
понимается период интегрирования показаний гироскопа). Авторами предлагается алгоритм, позволяющий 
уменьшить ошибку оцененной пространственной ориентации до минимальной величины, которая определяется 
точностью акселерометров, за 10 итераций вне зависимости от начального положения системы. 
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Коррекция оценки ориентации приборного базиса. В монографии [3] авторы предлагают построение 
модели коррекции в векторной форме с помощью плоскости приведения, ориентиром для построения которой 
является вектор силы тяжести. Для построения полного приборного базиса нужны два неколлинеарных 
ориентира. В качестве второго ориентира может выступать вектор магнитного поля. В данной работе 
рассматривается исключительно коррекция на базе плоскости приведения. 

Вектор при описании вращения кватернионами удобно выражать как кватернион, нулевая компонента 
которого (скалярная часть кватерниона) равна нулю 

{+ = (0, Ассех, Ассё,, Ассе[»), (3) 
где {;°— вектор оценки линейных ускорений в связанном базисе, выраженный через кватернион; Ассех — 
проекция оценки линейного ускорения на ось Х акселерометра в связанном базисе; Ассе, — проекция оценки 
линейного ускорения на ось У акселерометра в связанном базисе; Ассе[, — проекция оценки линейного 
ускорения на ось 7 акселерометра в связанном базисе. 

Необходимо выполнить проекцию вектора оценки линейных ускорений из связанного базиса в 
опорный [4]: 

п" = Лой" о А, (4) 
где й” — вектор показаний акселерометра в опорном базисе, выраженный через кватернион; Л — кватернион 
текущей пространственной ориентации; А — сопряженный кватернион текущей пространственной ориентации. 

Умножение кватернионов не коммутативно, обозначается символом «о» и выражается системой 
уравнений (6-9) [5]: 


М = ЛэМ, (5) 
№ = ЛоМо - А1 М, -— А2М» — АзМз, (6) 
М: = ЛоМ; + А. Мо + А2М: -— АзМЬ, (7) 
М. = ЛоМ> + Л>Мо + Аз М, -— А. М-, (8) 
М: = ЛоМз + АзМо + А: М) -— А>М:. (9) 


Кватернионом, сопряженным данному кватерниону Л, является кватернион, обозначаемый Л [6]: 
А = (№, -№М, -№, - №3). (10) 


У сопряженного кватерниона векторная часть взята с обратным знаком (данный кватернион необходим 
для осуществления проекций из одного базиса в другой). 
Вектор ориентира в опорном базисе выразим через кватернион: 


й = (0, 0, 0, 1), (10 


где и — вектор ориентира в опорном базисе в выбранной системе координат, выраженный через кватернион 
(рис. 1). 

В данном случае в качестве неподвижного ориентира выступает вектор силы тяжести в опорном 
базисе, так как выбор данного ориентира позволяет скомпенсировать ошибку ориентации по крену и тангажу. 
Компоненты вектора ориентира выбираются из следующих соображений: 

® нулевой компонент равен нулю; 


® |-Й компонент — показание акселерометра по оси Х при совмещении осей датчика с опорным 
базисом; 

® 2-Й компонент — показание акселерометра по оси У при совмещении осей датчика с опорным 
базисом; 

® 3-Й компонент — показание акселерометра по оси Й при совмещении осей датчика с опорным 
базисом. 


Разница между вектором ориентира в опорном базисе и вектором оценки линейных ускорений В 
опорном базисе составит величину 


[6 = й = И. (12) 


Компоненты коррекции оценки пространственной ориентации В опорном базисе находим через 
формулу: 
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ор = бой", (13) 


где ©; — кватернион, содержащий компоненты коррекции кватерниона пространственной ориентации в 
опорном базисе. 
Проекция компонент коррекции из опорного в связанный базис выражается через формулу 


в" = Лото Л, (14) 


где «р — кватернион, содержащий компоненты коррекции кватерниона пространственной ориентации в 
связанном базисе. 
С помощью (0+ корректируется оценка угловой скорости: 








ДА = (1, эт Хо оЕБУАЕ ср, ру ева) 


| п вме), (15) 


„5 2 
где «х — проекция угловой скорости на ось Х гироскопа в связанном базисе; «у — проекция угловой скорости 
на ось У гироскопа в связанном базисе; а, — проекция угловой скорости на ось 7 гироскопа в связанном 
базисе; ЛЕ — период интегрирования показаний гироскопа; © — коэффициент пропорциональной коррекции, 
© > 0; ДЛ — кватернион поворота. 

Коэффициент © выбирается из следующих соображений. При большом значении коэффициента 
возникает колебательный процесс компонент кватерниона, при малом значении коррекция не будет 
компенсировать ошибку, вызванную разного рода погрешностями при измерении угловой скорости, поэтому 
данный коэффициент выбирается из компромисса скорости приведения приборного базиса к связанному и 
отсутствия колебательного процесса. 

Авторы в [2] вводят понятие кватерниона малого поворота. Если период опроса датчика очень мал, 
система физически не успеет повернуться на большой угол за отведенное время. С учетом этого целесообразно 
отказаться от функции вычисления синуса угла (тем самым уменьшив процессорное время расчета), т. к. 
функция синуса вблизи нуля почти линейна. В этом случае осуществляется линейная аппроксимация 
кватерниона поворота: 





®«х-(а: д. ))-АЕ (шу-(а: в): (шл-(а: вез). Е 
АЛ = (1, @Х Е1)) с Е?) ‚ @2( Ез)) ). (16) 
2 2 2 
Затем выполняется операция умножения кватерниона текущей пространственной ориентации на 
кватернион малого поворота: 


М, = М1 ° АА, (17) 


где № — кватернион оценки пространственной ориентации; М№,_-, — кватернион оценки пространственной 
ориентации в предыдущий момент времени. 

С учетом медленного ухода нормы кватерниона от единицы при реализации вычислений на ЭВМ, 
необходимо периодически производить нормирование кватерниона пространственной ориентации по формулам 
(19, 20), т. е. приводить норму кватерниона к единице: 


ИМ |= уд + у2 + у2 + 2 = 1, (18) 


где || № | — норма кватерниона; 


[М = Мб + 2 + 92 + м2, (19) 


где |№| — тензор кватерниона; 
где Ммо’т — нормированный кватернион или верзор [1, 6]. 

Описанный выше алгоритм (3-20) осуществляет устойчивую коррекцию  кватерниона 
пространственной ориентации через изменение оценки угловой скорости так же, как это происходит в 
алгоритмах Махони [8,10] и Маджвика [4]. Разница алгоритмов заключается в методике определения 
компонентов коррекции. 

Алгоритм быстрой начальной инициализации кватерниона пространственной ориентации. 
Целью предлагаемого алгоритма является уменьшение времени приведения приборного базиса к связанному, 
причем коррекция осуществляется итерационно, посредством умножения кватерниона пространственной 
ориентации на кватернион коррекции. 
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Последовательность действий в алгоритме следующая: 

1) Опрос трех осей акселерометра; 

2) Определение компонент коррекции оценки ориентации по формулам (3-14); 
3) Преобразование компонент коррекции в кватернион поворота: 





ДА = (1 зтеь, зп Ре, зп), (21) 


где 1 — первая компонента коррекции в связанном базисе; > — вторая компонента коррекции в 

связанном базисе; дз — третья компонента коррекции в связанном базисе. 

4) Кватернион текущей пространственной ориентации умножается на сопряженный кватернион 
поворота: 


М, = М, 1 ° ДА. (22) 


5) Выполняется нормирование кватерниона пространственной ориентации (19, 20); 
6) Возврат к шагу 1. 


Эксперимент по определению времени переходных процессов компонент кватерниона при 
использовании разработаннгого алгоритма. Цель эксперимента — сравнить время переходных процессов 
компонент кватерниона при использовании коррекции через изменение оценки угловой скорости и коррекции 
по предложенному алгоритму. Описанные в данной статье алгоритмы приведения приборного базиса к 
связанному были реализованы на микроконтроллере &Р/С33ЗЕР256МИ806. В качестве датчика для определения 
пространственной ориентации использовался МРИ6050 — 3-х осевой гироскоп, измеряющий проекции 
угловых скоростей на свои оси чувствительности, и 3-х осевой акселерометр. Микроконтроллером вычислялись 
компоненты трех кватернионов пространственной ориентации по показаниям датчика при однократном 
измерении. Компоненты первого кватерниона вычислялись через изменение оценки угловой скорости с 
коэффициентом & = 0,1 (предварительный эксперимент со многими измерениями показал, что именно это 
значение является компромиссом между колебательным процессом и скоростью коррекции приборного базиса 
в течении всего времени работы данной системы ориентации). Компоненты второго кватерниона вычислялись 
через изменение оценки угловой скорости с увеличенным коэффициентом & = 5. Компоненты третьего 
кватерниона вычислялись по предложенному алгоритму быстрой инициализации. В момент включения и на 
протяжении всего эксперимента, датчик находился повернутым относительно горизонта по тангажу на 18 
градусов и по крену на 174 градуса. 


Пропорциональная коррекция кватерниона а = 0,1 
т т Т 
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Значение компонент кватерниона 
Гаше офдишегтоп сотропепий5 15 4едепиеа 


Л№ итфег о иеганоп5 15 аеяепие 


Рис. 2. Переходные процессы компонент кватерниона в параметрах Родрига-Гамильтона. 


Ех. 2. Тгапзйоп ргосе55ез оГдижегтоп сотропепт5 т Ко4иие;-НатШоп рагатеегу 


До момента времени &% = 50 микроконтроллер отправляет 3 единичных кватерниона для обработки в 
программу, работающую в интерпретируемой среде МаНаБ. В момент времени &% включается коррекция. В 
первом случае приведение приборного базиса к связанному завершается в момент времени #1 = 4415. Во 
втором случае — в момент &› = 150. В третьем случае — в момент #5 = 55. Признаком окончания коррекции 
является завершение переходного процесса компонент кватерниона: 
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Д; диаг! = Ы — © = 4365, (23) 
Д; она = &> — ® = 100, (24) 
Д; диав= В -®=5, (25) 


где ДЕ она — количество итераций, за которое система ориентации скорректировала оценку пространственной 
ориентации в компонентах кватерниона в каждом из трех случаев. 


Выводы. Предложенный алгоритм начальной выставки системы оказался быстрей в 873 раза по 
сравнению с коррекцией через оценку угловой скорости с коэффициентом коррекции & = 0,1 и в 20 раз 
быстрей по сравнению с коррекцией через оценку угловой скорости с увеличенным коэффициентом коррекции 
(= 5. В силу того, что кватернион поворота в данном алгоритме вычисляется только по показаниям 
акселерометров, то ошибка выставки системы будет прямо пропорциональна паразитному ускорению, 
действующему на систему. Под паразитным следует понимать любое ускорение, отличное от ускорения 
свободного падения. Поэтому после завершения начальной «выставки» необходимо отключать предложенный 
алгоритм и включать коррекцию через изменение оценки угловой скорости, как это предлагается в работах [4, 8]. 
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